#include "ServoUbt.h"


CServoUBT::CServoUBT()
{
}

CServoUBT::~CServoUBT()
{
}

bool CServoUBT::IsOffsetSupport()
{
	return true;
}

ServoRet_t CServoUBT::SetAngle(uint16_t angle, uint32_t msec)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackSetAngle(sndSize, snd, m_id, angle, msec, msec)) {
		return SERVO_RET_ERROR;
	}
	
	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 1, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackSetAngle(1, rcv, m_id)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::GetAngle(uint16_t &angle)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackGetAngle(sndSize, snd, m_id)) {
		return SERVO_RET_ERROR;
	}

	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 10, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	uint8_t angleu8;
	if (!CUBTServoProtocol::UnpackGetAngle(10, rcv, m_id, &angleu8)) {
		return SERVO_RET_ERROR;
	}
	
	angle = angleu8;
	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::SetOffset(int16_t offset)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackSetOffset(sndSize, snd, m_id, offset)) {
		return SERVO_RET_ERROR;
	}

	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 10, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackSetOffset(10, rcv, m_id)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::GetOffset(int16_t &offset)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackGetOffset(sndSize, snd, m_id)) {
		return SERVO_RET_ERROR;
	}
	
	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 10, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackGetOffset(10, rcv, m_id, &offset)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::SetId(uint8_t newid)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackSetId(sndSize, snd, m_id, newid)) {
		return SERVO_RET_ERROR;
	}
	
	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 10, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackSetId(10, rcv, newid)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::SetLed(bool on)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackSetLed(sndSize, snd, m_id, on)) {
		return SERVO_RET_ERROR;
	}

	// Send and ecv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 1, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackSetLed(1, rcv, m_id)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::Terminate()
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackTerminate(sndSize, snd, m_id)) {
		return SERVO_RET_ERROR;
	}
	
	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 1, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackTerminate(1, rcv, m_id)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::GetFirmware(uint32_t &firmware)
{
	// Pack
	uint8_t snd[UBT_SERVO_CMD_SIZE];
	int sndSize = UBT_SERVO_CMD_SIZE;
	if (!CUBTServoProtocol::PackGetFirmware(sndSize, snd, m_id)) {
		return SERVO_RET_ERROR;
	}
	
	// Send and rcv
	uint8_t rcv[UBT_SERVO_CMD_SIZE];
	ServoRet_t ret = sendCommand(snd, 10, rcv);
	if (SERVO_RET_SUCCESS != ret) {
		return ret;
	}

	// Unpack
	if (!CUBTServoProtocol::UnpackGetFirmware(10, rcv, m_id, firmware)) {
		return SERVO_RET_ERROR;
	}

	return SERVO_RET_SUCCESS;
}

ServoRet_t CServoUBT::Lock()
{
	uint16_t angle;
	ServoRet_t ret = GetAngle(angle);
	if (ret != SERVO_RET_SUCCESS) {
		return ret;
	}
	
	return SetAngle(angle, 0);
}

ServoRet_t CServoUBT::Unlock()
{
	uint16_t angle;
	return GetAngle(angle);
}

void CServoUBT::prepareSnd()
{
#ifdef TXEN_PIN
	digitalWrite(TXEN_PIN, LOW);
#endif
}

void CServoUBT::prepareRcv()
{
#ifdef TXEN_PIN
	digitalWrite(TXEN_PIN, HIGH);
#endif
}

ServoRet_t CServoUBT::sendCommand(uint8_t *cmd, int expect, uint8_t *out)
{
	// Flush rcv buffer before sending...
	prepareRcv();
	while (m_serlPtr->available()) {
		m_serlPtr->read();
	}

	prepareSnd();
	m_serlPtr->write(cmd, UBT_SERVO_CMD_SIZE);
	m_serlPtr->flush();
	prepareRcv();
	if (expect > 0) {
		int count = 0;
		unsigned long mwait = millis() + m_waitMsec;
		while ((count < expect) && (millis() < mwait))
		{
			if (m_serlPtr->available()) {
				out[count++] = m_serlPtr->read();
			}
			else {
				delay(1);
			}
		}

		if (count < expect) {
			return SERVO_RET_TIMEOUT;
		}
	}

	return SERVO_RET_SUCCESS;
}


// CUBTServoExec
CUBTServoExec::CUBTServoExec()
{
}

CUBTServoExec::~CUBTServoExec()
{
}

ServoRet_t CUBTServoExec::Execute(HardwareSerial *serl, uint8_t *cmd, int &expect, uint8_t *out)
{
	// Checksum
	uint8_t sum = CUBTServoProtocol::Checksum(cmd);
	if (sum != cmd[8] || 0xED != cmd[9]) {
		return SERVO_RET_ERROR;
	}

	// Command identify
	bool support = false;
	expect = 0;
	if (cmd[0] == 0xFA && cmd[1] == 0xAF) {
		switch(cmd[3])
		{
		case 0x01: // set angle/terminate
			if (cmd[4] == 0xFF) {
				expect = 1;
			}
			else {
				expect = 1;
			}
			support = true;
			break;
		case 0x02: // get angle
			support = true;
			expect = 10;
			break;
		case 0xD2: // set offset
			support = true;
			expect = 10;
			break;
		case 0xD4: // get offset
			support = true;
			expect = 10;
			break;
		case 0x04: // set led
			support = true;
			expect = 1;
			break;
		case 0xCD: // set id
			support = true;
			expect = 10;
			break;
		default:
			support = false;
			expect = 0;
			break;
		}
	}
	else if (cmd[0] == 0xFC && cmd[1] == 0xCF) {
		if (cmd[3] == 0x01) {
			support = true;
			expect = 10;
		}
		else {
			support = true;
			expect = 0;
		}
	}
	else {
		support = false;
		expect = 0;
	}
	if (!support) {
		return SERVO_RET_UNSUPPORT;
	}

	// Flush rcv buffer before sending...
#ifdef TXEN_PIN
	digitalWrite(TXEN_PIN, HIGH);
#endif
	while (serl->available()) {
		serl->read();
	}

	// Send command and receive as needed
#ifdef TXEN_PIN
	digitalWrite(TXEN_PIN, LOW);
#endif
	serl->write(cmd, UBT_SERVO_CMD_SIZE);
	serl->flush();
#ifdef TXEN_PIN
	digitalWrite(TXEN_PIN, HIGH);
#endif
	if (expect > 0) {
		int count = 0;
		unsigned long mwait = millis() + 10; // 10ms
		while ((count < expect) && (millis() < mwait))
		{
			if (serl->available()) {
				out[count++] = serl->read();
			}
			else {
				delay(1);
			}
		}

		if (count < expect) {
			return SERVO_RET_TIMEOUT;
		}
	}

	return SERVO_RET_SUCCESS;
}
